// Autonomous Code

void autonomousOne() { // Red Autonomous
  int timerA;
  SensorValue[ArmEncoder] = 0;
  SensorValue[LeftEncoder] = 0;
  SensorValue[RightEncoder] = 0;
  timedMove(-60,25); // Move back, preload
  //clawState = 4; // Open claw
  wait10Msec(200);
  changeArmStateW(2); // Flip claw
  wait10Msec(70);
  changeArmStateW(1);
  wait10Msec(50);
  correctWheels(200);
  wait10Msec(30);
  moveTilTubeW(500); // Get tubes stack 1
  if (clawState != 1) { // Claw close failsafe
    clawState = 2;
    wait10Msec(120);
  }
  changeArmStateW(3);
  wait10Msec(50);
  turnTP(-160);
  wait10Msec(50);
  timedMove(40,70);
  wait10Msec(50);
	timerA = 0;
	while ((SensorValue[Ultrasonic] > 70 || SensorValue[Ultrasonic] == -1) && timerA < 240) {
    driveState = 4;
	  wheelLeftPower = minMovePower + 5;
    wheelRightPower = minMovePower + 5;
    wait1Msec(10);
    timerA += 1;
	}
	wheelLeftPower = -10;
  wheelRightPower = -10;
  driveState = 3;
  wait10Msec(30);
  moveForwardTP(-235);
  scoreArm(2); // Score stack 1
  clawState = 4;
  moveForwardTP(-150);
  changeArmStateW(3);
  turnTP(-540);
  changeArmStateW(1);
  timedMove(50,30);
  moveTilTubeW(330); // Get tubes stack 2
  if (clawState != 1) { // Claw close failsafe
    clawState = 2;
    wait10Msec(120);
  }
  wait10Msec(50);
}

void autonomousTwo() { // Blue Autonomous
  int timerA;
  SensorValue[ArmEncoder] = 0;
  SensorValue[LeftEncoder] = 0;
  SensorValue[RightEncoder] = 0;
  timedMove(-60,25); // Move back, preload
  //clawState = 4; // Open claw
  wait10Msec(200);
  changeArmStateW(2); // Flip claw
  wait10Msec(70);
  changeArmStateW(1);
  wait10Msec(50);
  correctWheels(200);
  wait10Msec(30);
  moveTilTubeW(500); // Get tubes stack 1
  if (clawState != 1) { // Claw close failsafe
    clawState = 2;
    wait10Msec(120);
  }
  changeArmStateW(3);
  wait10Msec(50);
  moveForwardTP(100);
  turnTP(180);
  wait10Msec(50);
  timedMove(40,70);
  wait10Msec(50);
	timerA = 0;
	while ((SensorValue[Ultrasonic] > 70 || SensorValue[Ultrasonic] == -1) && timerA < 210) {
    driveState = 4;
	  wheelLeftPower = minMovePower + 5;
    wheelRightPower = minMovePower + 5;
    wait1Msec(10);
    timerA += 1;
	}
	wheelLeftPower = 0;
  wheelRightPower = 0;
  driveState = 3;
  wait10Msec(30);
  moveForwardTP(-235);
  scoreArm(2); // Score stack 1
  clawState = 4;
  moveForwardTP(-150);
  changeArmStateW(3);
  turnTP(540);
  changeArmStateW(1);
  timedMove(50,30);
  moveTilTubeW(330); // Get tubes stack 2
  if (clawState != 1) { // Claw close failsafe
    clawState = 2;
    wait10Msec(120);
  }
  wait10Msec(50);
}

void autonomousThree() { // Autonomous 2
  changeArmStateW(2); // Raise arm, drop claw
  wait10Msec(100);
  changeArmStateW(1); // Lower arm
  wait10Msec(30);
  timedMove(60,20);
  moveTilTubeW(300); // Move to tubes
  wait10Msec(30);
  /*timedTurn(-50,60); // Robot Turn
  wait10Msec(50);
  timedMove(50,90);*/
}

/*void autonomousFour() {

}*/

void autonomousFive() { // Descore, score
  int timerA = 0;
  SensorValue[ArmEncoder] = 0;
  SensorValue[LeftEncoder] = 0;
  SensorValue[RightEncoder] = 0;
  if (clawState != 5) {
	  clawState = 4;
	  wait10Msec(10);
	}
  armState = 2;
  wait10Msec(30);
  timedMove(-100,10); // Move back, preload, flip claw
  wait10Msec(30);
  changeArmStateW(1);
  clawState = 4;
  correctWheels(200);
  moveForwardTP(230);
  moveTilTubeW(400); // Get tubes stack 1
  /*if (clawState != 1) { // Claw close failsafe
    clawState = 2;
    wait10Msec(120);
  }*/
  armState = 7;
  wait10Msec(20);
  moveForwardTP(-55);
  wait10Msec(20);
  turnTP(100);
  moveForwardTP(650);
  armState = 3;
  wait10Msec(50);
  ultraLowMove(-204);
  scoreArm(2);
  clawState = 4;
  moveForwardTP(-160);
}

/*void autonomousSix() {

}

void autonomousSeven() {

}

void autonomousEight() {

}

void autonomousNine() {

}

void autonomousTen() {

}*/

void autonomousEleven() {
  int timerA = 0;
  SensorValue[ArmEncoder] = 0;
  SensorValue[LeftEncoder] = 0;
  SensorValue[RightEncoder] = 0;
  if (clawState != 5) {
	  clawState = 4;
	  wait10Msec(10);
	}
  armState = 2;
  wait10Msec(30);
  timedMove(-100,10); // Move back, preload, flip claw
  wait10Msec(30);
  changeArmStateW(1);
  clawState = 4;
  correctWheels(200);
  moveForwardTP(230);
  moveTilTubeW(400); // Get tubes stack 1
  armState = 7;
  wait10Msec(20);
  moveForwardTP(-55);
  wait10Msec(20);
  turnTP(-100);
  moveForwardTP(650);
  armState = 3;
  wait10Msec(50);
  ultraLowMove(-204);
  scoreArm(2);
  clawState = 4;
  moveForwardTP(-160);
}

/*void autonomousTwelve() {

}

void autonomousThirteen() {

}

void autonomousFourteen() {

}*/

void autonomousFifteen() { // Ram - reverse, claw flip
  moveForwardTP(-250);
  turnTP(-210);
  timedMove(-110,150);
  wait10Msec(50);
  clawState = 4;
  wait10Msec(10);
  armState = 2;
  wait10Msec(80);
  armState = 1;
  moveForwardTP(400);
}

void programChallenge() { // Programming Skill Challenge
  int timerA = 0;
  SensorValue[ArmEncoder] = 0;
  SensorValue[LeftEncoder] = 0;
  SensorValue[RightEncoder] = 0;
  if (clawState != 5) {
	  clawState = 4;
	  wait10Msec(30);
	}
  armState = 2;
  wait10Msec(30);
  timedMove(-100,10); // Move back, preload, flip claw
  wait10Msec(30);
  changeArmStateW(1);
  clawState = 4;
  correctWheels(200);
  moveForwardTP(230);
  moveTilTubeW(400); // Get tubes stack 1
  armState = 3;
  wait10Msec(20);
  moveForwardTP(-55);
  wait10Msec(20);
  turnTP(110);
  moveForwardTP(405); //forward
  ultraLowMove(-225);
  scoreArm(2); // Score 1
  // ----------------------- 2 --------------------------
  clawState = 4;
  wait10Msec(30);
  if (clawState != 5) {
    clawState = 4;
    wait10Msec(30);
  }
  armState = 3;
  wait10Msec(100);
  moveForwardTP(-20);
  turnTP(560);
  armState = 1;
  wait10Msec(40);
  moveForwardTP(600);
  moveTilTubeW(200);
  armState = 3;
  wait10Msec(20);
  turnTP(53);
  wait10Msec(30);
  moveForwardTP(550, 0.3);
  ultraLowMove(-225, 8);
  scoreArm(2); // Score 2
  clawState = 4;
  wait10Msec(30);
  if (clawState != 5) {
    clawState = 4;
    wait10Msec(30);
  }
  // ---------------------- 3 --------------------
  armState = 3;
  wait10Msec(30);
  moveForwardTP(-180);
  turnTP(-153);
  armState = 1;
  moveForwardTP(1150);
  clawState = 4;
  moveTilTubeW(300); // Grab 3
  armState = 3;
  moveForwardTP(-430);
  wait10Msec(30);
  turnTP(275);
  moveForwardTP(170);
  ultraLowMove(-225);
  scoreArm(2); // Score 3
  clawState = 4;
  wait10Msec(30);
  if (clawState != 5) {
    clawState = 4;
    wait10Msec(30);
  }
  // ---------------------- 4 -------------------------
  moveForwardTP(-65);
  armState = 3;
  wait10Msec(50);
  turnTP(-300);
  armState = 1;
  moveForwardTP(1400);
  moveTilTubeW(400); // Grab 4
  armState = 4;
  moveForwardTP(-190);
  wait10Msec(30);
  turnTP(238);
  timerA = 0;
	while ((SensorValue[Ultrasonic] > 343 || SensorValue[Ultrasonic] == -1) && timerA < 390) {
    driveState = 4;
	  wheelLeftPower = minMovePower + 5;
    wheelRightPower = minMovePower + 5;
    wait1Msec(10);
    timerA += 1;
	}
	wheelLeftPower = -10;
  wheelRightPower = -10;
  driveState = 3;
  armState = 9; // Score 4
  wait10Msec(80);
  clawState = 4;
  wait10Msec(30);
  // ------------------------------- 5 -----------------------------------------
  if (clawState != 5) {
    clawState = 4;
    wait10Msec(30);
  }
  moveForwardTP(-760);
  turnTP(-320);
  changeArmStateW(1);
  moveForwardTP(800);
  moveTilTubeW(200); // Grab stack 5
  armState = 3;
  moveForwardTP(-525);
  turnTP(200);
  moveForwardTP(300);
  ultraLowMove(-225);
  scoreArm(2); // Score stack 5
  clawState = 4;
  wait10Msec(30);
  if (clawState != 5) {
    clawState = 4;
    wait10Msec(30);
  }
  // --------------------------------- 6 -------------------------------
  timedMove(-46,180);
  wait10Msec(30);
  turnTP(-300);
  changeArmStateW(1);
  moveForwardTP(250);
  moveTilTubeW(600); // grab 6
  armState = 4;
  moveForwardTP(260);
  turnTP(-300);
  moveForwardTP(350);
  ultraLowMove(-225);
  scoreArm(2);
  clawState =4;
  wait10Msec(30);
  if (clawState != 5) {
    clawState = 4;
    wait10Msec(30);
  }
  moveForwardTP(-160);
}
